package com.hitqz.robot.driver.zmq.protocol.handler;

import com.example.tutorial.AddressBookProtos;
import com.hitqz.robot.api.common.entity.RobotMapPos;
import com.hitqz.robot.driver.communication.NetworkServiceThreadPool;
import com.hitqz.robot.driver.dto.PoseDto;
import com.hitqz.robot.driver.util.QueueStatusEnum;
import com.hitqz.robot.driver.util.WheelCacheUtil;
import com.hitqz.robot.driver.util.WheelToolkit;
import lombok.extern.slf4j.Slf4j;
import org.springframework.stereotype.Component;

import java.util.List;
import java.util.Map;
import java.util.Objects;

/**
 * 导航队列状态处理类-巡线、二维码、磁导航
 *
 * @author wengyx
 * @date 2021-09-14 18:21
 **/
@Slf4j
@Component
public class TaskStatusHandler implements HandlerZMQService {

    private int lastStatus = -1;

    @Override
    public void handlerMsg(String robotSn, byte[] data) throws Exception {
        AddressBookProtos.TaskStatus taskStatus = AddressBookProtos.TaskStatus.parseFrom(data);
        String type = taskStatus.getDescriptorForType().getName();
        int objId = taskStatus.getHeader().getObj();
        int ackId = taskStatus.getHeader().getAck();
        int status = taskStatus.getStatus();
        String taskId = taskStatus.getTaskId();
        long stamp = taskStatus.getHeader().getStamp();
        switch (objId) {
            case AddressBookProtos.TaskStatus.OBJ_ID.GET_TASK_STATUS_REP_VALUE,
                 AddressBookProtos.TaskStatus.OBJ_ID.NUL_TASK_STATUS_NTF_VALUE -> {
                if (!Objects.equals(WheelToolkit.currentTaskId, taskId)){
                    //不是当前任务的状态不做处理
                    log.warn("[wheel]not current task.task_id:{},status:{}", taskId, QueueStatusEnum.ROBOT_QUEUE_STATUS.get(status));
                    break;
                }
                log.info("[wheel]task_id:{},status:{}", taskId, QueueStatusEnum.ROBOT_QUEUE_STATUS.get(status));
                if (Objects.equals(taskStatus.getStatus(),QueueStatusEnum.OBSTACLE.getKey())){
                    //激光避障
                    //todo 判断30s内不动才报警，并关闭任务

                }
                // 去重状态
                if (lastStatus == status){
                    break;
                }
                lastStatus = status;
                // 保存信息
                WheelCacheUtil.put(WheelCacheUtil.KEY_TASK_QUEUE_STATUS, status);
                if (Objects.equals(taskStatus.getStatus(), QueueStatusEnum.OVER.getKey())) {
                    //重置当前队列id
                    WheelToolkit.currentTaskId =  "";
                    //移除完成的队列
                    WheelToolkit.taskPosQueue.poll();
                    PoseDto currentPose = (PoseDto) WheelCacheUtil.get(WheelCacheUtil.KEY_CURR_POSE);
                    log.info("[wheel]reach point. ({},{}) yaw:{}",currentPose.getX(),currentPose.getY(),currentPose.getYaw());
                    //处理下一个队列
                    NetworkServiceThreadPool.dealEvent(WheelToolkit::dealTaskPosQueue);
                }
                if (Objects.equals(taskStatus.getStatus(), QueueStatusEnum.LOST.getKey())){
                    //定位丢失
                    // todo 告警，并关闭任务
                }
                if (Objects.equals(taskStatus.getStatus(),QueueStatusEnum.DERAIL.getKey())){

                    //脱轨
                    // todo 告警，并关闭任务
                }
            }

        }
        //释放锁
        WheelToolkit.release(ackId,type, objId, true);

    }






}
